#include <deque>
#include "geometrydata.h"
#include "../math/Vector3.h"

using namespace PBVP;

GeometryData::GeometryData()
{
	m_NumVertices = 0;

	m_pVertInfo = NULL;
	m_pTriInfo = NULL;
}

GeometryData::~GeometryData()
{
	clearGeoInfo();
}

int GeometryData::addVertex(float x, float y, float z)
{
	m_VertexArray.push_back(x);
	m_VertexArray.push_back(y);
	m_VertexArray.push_back(z);
	m_NumVertices ++;
	return m_NumVertices-1;
}

int GeometryData::addVertex(const Vector3& vertex)
{
	m_VertexArray.push_back(vertex.x);
	m_VertexArray.push_back(vertex.y);
	m_VertexArray.push_back(vertex.z);
	m_NumVertices ++;
	return m_NumVertices-1;
}

int GeometryData::addVertex(const Vector3& vertex, const Vector3& normal)
{
	m_VertexArray.push_back(vertex.x);
	m_VertexArray.push_back(vertex.y);
	m_VertexArray.push_back(vertex.z);
	m_NormalArray.push_back(normal.x);
	m_NormalArray.push_back(normal.y);
	m_NormalArray.push_back(normal.z);
	m_NumVertices++;
	return m_NumVertices-1;
}

int GeometryData::addVertex(const Vector3& vertex, const Vector3& normal, const Vector3& color)
{
	m_VertexArray.push_back(vertex.x);
	m_VertexArray.push_back(vertex.y);
	m_VertexArray.push_back(vertex.z);
	m_NormalArray.push_back(normal.x);
	m_NormalArray.push_back(normal.y);
	m_NormalArray.push_back(normal.z);
	m_ColorArray.push_back(color.x);
	m_ColorArray.push_back(color.y);
	m_ColorArray.push_back(color.z);
	m_NumVertices++;
	return m_NumVertices-1;
}

int GeometryData::addNormal(const Vector3& norm)
{
	m_NormalArray.push_back(norm.x);
	m_NormalArray.push_back(norm.y);
	m_NormalArray.push_back(norm.z);
	return (m_NormalArray.size()/3 - 1);
}

int GeometryData::addColor(const Vector3& color)
{
	m_ColorArray.push_back(color.x);
	m_ColorArray.push_back(color.y);
	m_ColorArray.push_back(color.z);
	return (m_ColorArray.size()/3 -1);
}

int GeometryData::addTriangle(int index[3])
{
	m_TriangleIndexArray.push_back(index[0]);
	m_TriangleIndexArray.push_back(index[1]);
	m_TriangleIndexArray.push_back(index[2]);
	return numOfTriangles()-1;
}

int GeometryData::addTriangle(int vx, int vy, int vz)
{
	m_TriangleIndexArray.push_back(vx);
	m_TriangleIndexArray.push_back(vy);
	m_TriangleIndexArray.push_back(vz);
	return numOfTriangles()-1;
}

void GeometryData::calcNormals(bool forced)
{
	if(!forced && m_VertexArray.size() == m_NormalArray.size()) return;

	m_NormalArray.resize(3*numOfVertices());
	
	QVector<Vector3> vertexNormals;		// temp structure for vertex normals
	vertexNormals.resize(3*numOfVertices());

	//compute triangle normals
	for (int i = 0; i < numOfTriangles(); i++) {
		int t0, t1, t2;		// indices of three vertice of the triangle
		t0 = m_TriangleIndexArray[3*i];
		t1 = m_TriangleIndexArray[3*i + 1];
		t2 = m_TriangleIndexArray[3*i + 2];
		Vector3 v1(m_VertexArray[3*t1]-m_VertexArray[3*t0], 
				m_VertexArray[3*t1+1]-m_VertexArray[3*t0+1],
				m_VertexArray[3*t1+2]-m_VertexArray[3*t0+2]);
		Vector3 v2(m_VertexArray[3*t2]-m_VertexArray[3*t0],
					m_VertexArray[3*t2+1]-m_VertexArray[3*t0+1],
					m_VertexArray[3*t2+2]-m_VertexArray[3*t0+2]);
		Vector3 v3 = v1.crossProduct(v2);
		v3.normalize();
		vertexNormals[t0] += v3;
		vertexNormals[t1] += v3;
		vertexNormals[t2] += v3;
	}

	for (int i = 0; i < numOfVertices(); i++) {
		vertexNormals[i].normalize();
		m_NormalArray[3*i] = vertexNormals[i].x;
		m_NormalArray[3*i+1] = vertexNormals[i].y;
		m_NormalArray[3*i+2] = vertexNormals[i].z;
	}
}

void GeometryData::invertNormals()
{
	// don't change anything if there are no normals
	if (!hasNormalArray()) return;
	
	int n = 3 * numOfVertices();
	for (int i = 0; i < n; i++)
	{
		m_NormalArray[i] = -m_NormalArray[i];
	}
}

void GeometryData::invertFaceOrients()
{
	int n = numOfTriangles();
	for (int i = 0; i < n; i++) 
	{
		unsigned int tmp = m_TriangleIndexArray[3*i];
		m_TriangleIndexArray[3*i] = m_TriangleIndexArray[3*i+1];
		m_TriangleIndexArray[3*i+1] = tmp;
	}
}

void GeometryData::merge(GeometryData *pGeoData)
{
	bool mergeNormal = hasNormalArray() && pGeoData->hasNormalArray();
	bool mergeColor = hasColorArray() && pGeoData->hasColorArray();
	bool mergeTexCoord = hasTex2DArray() && pGeoData->hasTex2DArray();

	int count = 3*pGeoData->numOfVertices();
	for (int i = 0; i < count; i++) {
		m_VertexArray.push_back(pGeoData->m_VertexArray[i]);
		if (mergeNormal)
		{
			m_NormalArray.push_back(pGeoData->m_NormalArray[i]);
		}
		if (mergeColor)
		{
			m_ColorArray.push_back(pGeoData->m_ColorArray[i]);
		}
		if (mergeTexCoord)
		{
			m_TexCoordArray.push_back(pGeoData->m_TexCoordArray[i]);
		}
	}

	for(int i = 0; i < pGeoData->m_TriangleIndexArray.size(); i++) 
	{
		m_TriangleIndexArray.push_back(pGeoData->m_TriangleIndexArray[i]+m_NumVertices);
	}
	for (int i = 0; i < pGeoData->m_PointIndexArray.size(); i++)
	{
		m_PointIndexArray.push_back(pGeoData->m_PointIndexArray[i]+m_NumVertices);
	}
	for (int i = 0; i < pGeoData->m_LineIndexArray.size(); i++)
	{
		m_LineIndexArray.push_back(pGeoData->m_LineIndexArray[i]+m_NumVertices);
	}
	for (int i = 0; i < pGeoData->m_QuadIndexArray.size(); i++)
	{
		m_QuadIndexArray.push_back(pGeoData->m_QuadIndexArray[i]+m_NumVertices);
	}
	m_NumVertices += pGeoData->numOfVertices();
}

int GeometryData::orientTriangles()
{
	int nTri = numOfTriangles(), nc = 0;
	calcGeoInfo();
	bool *oriented = new bool[nTri]; 
	for (int i = 0; i < nTri; i++)
	{
		oriented[i] = false;
	}
	std::deque<int> queue;
	for (int i = 0; i < nTri; i++)
	{
		if (oriented[i] == false)
		{
			nc++;
			oriented[i] = true;		// i is the first triangle in the component
			if (!neighborsOriented(oriented, i)) {
				queue.push_back(i);
			}
			while(!queue.empty()) { // breadth-troversal 
				int t = queue.front();
				queue.pop_front();
				unsigned int vids[3];
				getTriangleIndex(t, vids);
				for(int j = 0; j < 3; j++) {	// consider neighoring triangles
					int t2 = getNeighborTriangle(t, j);
					if(t2 >= 0 && (!oriented[t2])) {
						int u, v;		// vertices shared by t and t2
						u = vids[(j+1)%3];
						v = vids[(j+2)%3];
						unsigned int vids2[3];
						getTriangleIndex(t2, vids2);
						int k;
						for (k = 0; k < 3; k++) {
							if(vids2[k] == u) break;
						}
						if (vids2[(k+1)%3] == v) {		// wrong orientation
#ifdef _DEBUG
							fprintf(stderr, "Wrong oritenation for triangle %d\n", t2);
#endif
							// swap the order of u and v for t2
							m_TriangleIndexArray[3*t2 + k] = v;
							m_TriangleIndexArray[3*t2 + (k+1)%3] = u;
						} else {	// right orientation
#ifdef _DEBUG
							assert(vids2[(k+2)%3] == v);
#endif
						}
						oriented[t2] = true;
						if(!neighborsOriented(oriented, t2)) {
							queue.push_back(t2);
						}
					}
				}
			}
		}
	}

	delete[] oriented;
	clearGeoInfo();
#ifdef _DEBUG
	printf("Number of connected components: %d\n", nc);
#endif
	return nc;
}

/****************************************************************************************/
/* Private functions																	*/
/****************************************************************************************/
void GeometryData::calcGeoInfo()
{
	// clear existing info first
	clearGeoInfo();

	int nvert = numOfVertices();
	int nface = numOfTriangles();
	m_pVertInfo = new VertexInfo[nvert];

	for(int i = 0; i < nface; i++) {
		unsigned int vids[3];
		getTriangleIndex(i, vids);
		for(int j = 0; j < 3; j++) {
			m_pVertInfo[vids[j]].addFaceNeighbor(i);
			m_pVertInfo[vids[j]].addVertNeighbor(vids[(j+1)%3]);
			m_pVertInfo[vids[j]].addVertNeighbor(vids[(j+2)%3]);
		}
	}
}

void GeometryData::clearGeoInfo()
{
	if(m_pVertInfo) {
		delete[] m_pVertInfo;
		m_pVertInfo = NULL;
	}
	if(m_pTriInfo) {
		delete[] m_pTriInfo;
		m_pTriInfo = NULL;
	}
}

int GeometryData::getNeighborTriangle(int n, int k)
{
	unsigned int vids[3];
	getTriangleIndex(n, vids);
	int v1 = vids[(k+1)%3];
	int v2 = vids[(k+2)%3];

	int valence = m_pVertInfo[v1].face_valence;
	for (int i = 0; i < valence; i++) {
		int tid = m_pVertInfo[v1].getFaceNeighbor(i);
		if (tid != n) {		// A different triangle other than n
			getTriangleIndex(tid, vids);
			for (int j = 0; j < 3; j++) {
				if(vids[j] == v2) return tid;
			}
		}
	}
	return -1;
}

bool GeometryData::neighborsOriented(bool oriented[], int n)
{
	for(int i = 0; i < 3; i++) {
		int t = getNeighborTriangle(n, i);
		if(t >= 0 && (!oriented[t])) {
			return false;
		}
	}
	return true;
}

void GeometryData::getExtent(Vector3& minExt, Vector3& maxExt) const
{
	minExt = getVertex(0);
	maxExt = getVertex(0);

	for (unsigned int i = 1; i < numOfVertices(); i++)
	{
		Vector3 vert = getVertex(i);
		for (int j = 0; j < 3; j++)
		{
			if (minExt[j] > vert[j]) minExt[j] = vert[j];
			if (maxExt[j] < vert[j]) maxExt[j] = vert[j];
		}
	}
}